Method of Determining the State of a Body

ABSTRACT

In a process and apparatus for determining the state of a body, a number n of measured values {right arrow over (x)} i  of a state of the body with i=1, . . . , n are measured and recorded, the measured values {right arrow over (x)} i  representing points in the k-dimensional space. The measured values {right arrow over (x)} i  of the state are then fed to a Kalman filter for estimating the state of the body. For each number n of measured values {right arrow over (x)} i  of a state of the body, a first quantity {right arrow over (m)} n  and a second quantity r n  are derived, and are fed to the Kalman filter, the quantity {right arrow over (m)} n  being the center vector, and the quantity r n  being the radius of a k-dimensional sphere B n  within which all points {right arrow over (x)} i  are situated.

BACKGROUND AND SUMMARY OF THE INVENTION

This application claims the priority of German patent document 10 2007002 672.4, filed Jan. 18, 2007, the disclosure of which is expresslyincorporated by reference herein.

The invention relates to a method of determining the physical state of abody.

For the control of vehicles, for example, motor vehicles and airplanes,electronic systems are increasingly used to supplement or partiallyreplace direct control by the driver or pilot. Examples of such systemsare found in antilock systems (ABS) and electronic vehicle stabilization(ESP).

Likewise, robots, whose control is increasingly taken over by electronicsystems, are used in manufacturing processes, for example, in theautomobile industry or for placement of components on printed circuitboards.

Furthermore, the state of a vehicle or airplane or the state of aplacement arm of a robot, is generally considered as the state of abody. In this definition, a body is a moving object, such as a vehicle(for example, a landcraft), an airplane, or an insertion arm of a robotwhich is suitable for taking up certain states.

The state of a body also includes its position, speed, attitude oradditional physical quantities, and is therefore generally defined byphysical quantities which can be measured by means of technical devices.Thus, the state of a body can also be characterized by the measurementof pressure, temperature or humidity.

For carrying out their function, systems such as mentioned above requireknowledge of the actual state that is as precise and reliable aspossible. For example, in the case of an airplane, in the simplest case,the state is characterized by its position, speed and orientation angle.The state of a surface mounted device can in the simplest case berepresented by the position of the robot arm. Depending on the modelingexpenditures, the state may comprise additional physical (that is,measurable) quantities. Since some relevant state variables are notaccessible for simple measurement, they partly must be estimated bymeans of models. In this case, the estimates can be coordinated bycomparison with measured values of observable quantities. However, it isproblematic that the modeled processes within the vehicle, or theposition of a robot arm, as well as the measurement of the controlquantities are subject to noise. Furthermore, some processes within thesystem to be observed are described by non-linear functions, so thattheir modeling requires considerable computing expenditures.

International Patent Document WO/1997/011334 A1 describes a navigationsystem for a vehicle which has a Kalman filter for estimatingcorrections for the navigation parameters and calibrating the sensors ofthe navigation system, and additional systems are known, for example,from European Patent Document EP 1 564 097 A1 or German Patent DocumentDE 10 2005 012 456 A1.

These known systems supply point quantities in a k-dimensional space asa measurement. A first group of such systems is characterized bymeasuring processes in which each measuring result is illustrated by apoint in a multidimensional space. A plurality of measuring results, asthey occur, for example, in the course of a series of measurements, isthen illustrated by a point cluster in a k-dimensional space. Theindividual points may also particularly represent measuring errors (thatis, deviations of the measuring results from a known true value). Asecond group is formed by processes in which, for the purpose ofscanning a k-dimensional space, this space is covered by a discretek-dimensional grid. Those grid points (in the case of a two-dimensionalimage processing, also pixels) at which the space to be scanned has acertain defined characteristic, are combined to a point quantity.

An example of the first group is the high-frequency recording of airdata of an airplane consisting of the static pressure and the dynamicpressure, or the angle of attack and the angle of yaw. Each measurementresults in a point in a two-dimensional space. Naturally, further airdata may be added, whereby the dimension of the space is increased.

Terrain Reference Navigation (TRN) process is an example of the secondgroup. Here, the two-dimensional horizontal plane or thethree-dimensional space is searched for points at which a predefinedminimum number of contour lines overlap. Surface-mounted device (SMD)placing machines represent another example of the second group, in whichprinted circuit boards are to be equipped with components at definedpositions (that is, at points of the two-dimensional plane).

To estimate the state, the point quantity is conventionally transmittedto a Kalman filter. However, this creates the disadvantage that theindividual measurements, which are represented by the point quantity,are generally considerably noise-infested, and because of the dataquantity to be processed by the Kalman filter, an output of theestimated state that is close with respect to the time is therefore notpossible.

One object to the invention is to provide a method which can achieve areduction of the noise, and can provide a precise estimation of thestate in real time.

This and other objects and advantages are achieved by the methodaccording to the invention, in which a number n of measured values{right arrow over (x)}_(i) of a state of a body are recorded,expediently by sensors which generate signals indicative of the measuredstate.

A first quantity {right arrow over (m)}_(n) and a second quantity r_(n)are computed according to the invention for each number n of measuredvalues {right arrow over (x)}_(i) of a state of a body. (In thiscontext, measured values {right arrow over (x)}_(i) include not only rawdata but also those values which have been processed inside or outside ameasuring device.) The computed quantities {right arrow over (m)}_(n)and r_(n) are fed to the Kalman filter for estimating the state of thebody and are further processed there, in which case the quantity {rightarrow over (m)}_(n) is the center and the quantity r_(n) is the radiusof a k-dimensional sphere B_(n), within which all points {right arrowover (x)}_(i) of the measurements of the respective state are situated.Because a large number n of measuring points are combined in the sphereB_(n), the noise of the center {right arrow over (m)}_(n) is clearlysuppressed with respect to the noise of the individual measuring points{right arrow over (x)}_(i).

By means of the process according to the invention, the center and theradius are determined for a k-dimensional sphere that is as small aspossible and which, without exception, contains all points {right arrowover (x)}_(i), i=1, . . . , n of the measured point quantity. Thissphere is described by its k-dimensional center {right arrow over(m)}_(n) and its radius r_(n).

These parameters are fed to a Kalman filter, which estimates an overallstate of the body to be examined, based on the series of measurements ofthe states of the individual sensors.

Other objects, advantages and novel features of the present inventionwill become apparent from the following detailed description of theinvention when considered in conjunction with the accompanying drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 shows an example of a two-dimensional distribution of a series ofmeasurements of measuring points {right arrow over (x)}_(i) of the stateof a body;

FIG. 2 is a view of a process for obtaining starting values {right arrowover (m)}₀ and r₀ for a first further development of the invention;

FIG. 3 is a view of an alternative process for obtaining starting values{right arrow over (m)}₀ and r₀ for a second further development of theinvention;

FIG. 4 is a view of the result of an optimization shown as an example;

FIG. 5 a is a view of the time sequence of TRN position measurementsshown as an example;

FIG. 5 b is a view of the time sequence of measurements of an inertiasensor shown as an example.

DETAILED DESCRIPTION OF THE DRAWINGS

In the process according to the invention, from a mathematical point ofview, the smallest sphere is determined which encloses the entire pointquantity {{right arrow over (x)}_(i), i=1, . . . , n}. This is theoptimization of a (k+1)-dimensional quantity under n secondaryconditions. Specifically, the k+1 quantities ({right arrow over(m)}_(n), r_(n)) are to be determined such that the radius r_(n) becomesminimal under the secondary conditions that |{right arrow over(x)}_(i)-{right arrow over (m)}_(n)|≦r_(n) for all i=1, . . . , n. Inthis case, |{right arrow over (x)}_(i)-{right arrow over (m)}_(n)|indicates the geometrical spacing of the two points {right arrow over(x)}_(i) and {right arrow over (m)}_(n) in the k-dimensional space. FIG.1 is a view of an example of a two-dimensional distribution of measuringpoints {right arrow over (x)}_(i).

In a first further embodiment of the invention, the process is dividedinto two steps: the first step being used to obtain starting values{right arrow over (m)}₀, r₀ for the subsequent optimization (secondstep).

To obtain the starting values, the minimal coordinate k_(min) _(—) _(y)and the maximal coordinate k_(max) _(—) _(y) of all points {right arrowover (x)}_(i) is determined in each of the k-dimensions. In this manner,the minimal k-dimensional cuboid is determined which contains all points{right arrow over (x)}_(i) (compare FIG. 2). The center vector of thethus obtained k-dimensional cuboid is called {right arrow over (m)}₀. Ofthe k edge lengths of the cuboid, the longest one is selected and halfof the latter is called r₀.

Starting from the center {right arrow over (m)}₀ and the radius r₀, thesubsequent optimization is advantageously recursively carried out asfollows:

It is assumed that the sphere around the center {right arrow over(m)}_(n) with the radius r₀ already contains all points {right arrowover (x)}_(i), with i≦v; that is, it is assumed that the relation|{right arrow over (x)}_(i)-{right arrow over (m)}_(v) |≦r_(v) hasalready been met for all i=1, . . . , v. If the relation |{right arrowover (x)}_(v+1)-{right arrow over (m)}_(v)|≦r_(v) also applies, {rightarrow over (m)}_(v+1)=m_(v) and r_(v+1)=r_(v) will be set. Otherwise,m_(v+1)=α·m_(v)+β·{right arrow over (x)}_(v+1) and r_(v+1)=α·51 {rightarrow over (x)}_(v+1)-{right arrow over (m)}_(v)| will be set, thecoefficients α and β being defined by

$\alpha = {{{\frac{1}{2} \cdot \left( {1 + \frac{r_{v}}{{{\overset{\rightarrow}{x}}_{v + 1} - {\overset{\rightarrow}{m}}_{v}}}} \right)}\mspace{20mu} {and}\mspace{20mu} \beta} = {\frac{1}{2} \cdot \left( {1 - \frac{r_{v}}{{{\overset{\rightarrow}{x}}_{v + 1} - {\overset{\rightarrow}{m}}_{v}}}} \right)}}$

By repeating the optimization step from v=0 to v=n, a sphere around thecenter {right arrow over (m)}_(n) with the radius r_(n) is finallyobtained which encloses all points {right arrow over (x)}_(i), i=1, . .. , n of the point quantity and meets all secondary conditions (FIG. 4).

In a second further embodiment of the invention, the average value{right arrow over (μ)} and the standard deviation σ of the pointquantity {right arrow over (x)}_(i) are determined for obtaining thestarting values {right arrow over (m)}₀, r₀. This alternativepossibility of obtaining starting values is shown in FIG. 3. In thiscase, the average value is computed according to

$\overset{\rightarrow}{\mu} = {\frac{1}{n} \cdot {\sum\limits_{i = 1}^{n}{\overset{\rightarrow}{x}}_{i}}}$

and the standard deviation is computed according to

$\sigma = {{\sqrt{\frac{1}{n}{\cdot {\sum\limits_{i = 1}^{n}{{{\overset{\rightarrow}{x}}_{i} - \overset{\rightarrow}{\mu}}}^{2}}}}\mspace{20mu} {or}\mspace{20mu} \sigma} = {\sqrt{\frac{1}{n - 1} \cdot {\sum\limits_{i = 1}^{n}{{{\overset{\rightarrow}{x}}_{i} - \overset{\rightarrow}{\mu}}}^{2}}}.}}$

The starting values for the following optimization are defined asfollows: {right arrow over (m)}₀={right arrow over (μ)} and r₀=σ.

The further recursive determination of the optimal center {right arrowover (m)}_(n) and of the radius r_(n) is obtained from the optimizationsteps as described above in the first further development of theinvention.

The invention will be explained below by means of a first example. Thestate determination and navigation of missiles is conventionally basedon the use of inertia systems, in which acceleration and rotationalspeed data are continuously measured by inertial sensors (accelerationsensors and rotational speed sensors), and are used to determinepositions, speeds and orientation angles of the missile. It is known,however, that a state determination or navigation which is basedexclusively on inertia systems has the disadvantage that the errors ofthe thus obtained state variables grow over time, becoming increasinglyless precise, and finally useless. This is illustrated in FIG. 5 b wherepoint M indicates the state (for example, the position) and circle Urepresents the inaccuracy of this state. At a later point in time(farther toward the right on the time scale), the inaccuracy hasincreased.

To avoid such a continuous deterioration of the accuracy, additionalsensors or processes, such as satellite navigation or Terrain ReferencedNavigation (TRN), are used with the inertial systems.

FIG. 5 a illustrates schematically the determination of the state of thebody by means of such an additional process. It is characteristic thatthe precision does not systematically deteriorate as time progresses.However, in contrast to an inertia system, the data are, as a rule, notcontinuously present.

It is known that the data supplied by the different sensors andprocesses can be combined in a Kalman filter, which uses an error modelto determine an optimal estimated value from the present data, which isgenerally more precise than the result of the individual sensors. Inthis manner, the defects of a pure inertia navigation are eliminated.This is illustrated in FIG. 5 b in that, by using the data of theadditional sensor or process from FIG. 5 a, the inaccuracy built up as aresult of the inertia navigation (compare second circle in FIG. 5 b) isreduced again (compare third circle in FIG. 5 b).

The invention can now be used, for example, in the case of the TerrainReferenced Navigation (TRN) at the interface to the Kalman filter. InTRN, a spatial area is determined which contains the two- orthree-dimensional position of the missile with a certain probability. Bythe quantization of the spatial area, the TRN outputs a two- orthree-dimensional point quantity. This point quantity is to be furtherprocessed in an integrated navigation system in real time, for which aKalman filter is typically used. However, the Kalman filter expects aposition (center) and a dimension (radius) as standard-type inputvariables. The described process is capable of processing the pointquantity supplied by the TRN such that subsequent Kalman filteringbecomes possible in real time.

The invention can therefore be used in a navigation system in which dataof the TRN are merged with data of an inertia system to form navigationsignals in a Kalman filter. Additional data (for example, from satellitenavigation signals) may flow into the Kalman filter.

The foregoing disclosure has been set forth merely to illustrate theinvention and is not intended to be limiting. Since modifications of thedisclosed embodiments incorporating the spirit and substance of theinvention may occur to persons skilled in the art, the invention shouldbe construed to include everything within the scope of the appendedclaims and equivalents thereof.

1. A method for determining the physical state of a body, said methodcomprising: measuring and recording of a number n of values {right arrowover (x)}_(i) of physical quantities that characterize the state of thebody, with i=1, . . . , n, the measured values {right arrow over(x)}_(i) representing points in the k-dimensional space; and feeding themeasured values {right arrow over (x)}_(i) to a Kalman filter forestimating the state of the body; wherein, for each number n of measuredvalues {right arrow over (x)}_(i), a first quantity {right arrow over(m)}_(n) and a second quantity r_(n) are derived, the quantity {rightarrow over (m)}_(n) being a center vector, and the quantity r_(n) beinga radius of a k-dimensional sphere B_(n) within which all points {rightarrow over (x)}_(i) are situated, with i=1, . . . , n; and the derivedquantities {right arrow over (m)}_(n) and r_(n) are fed to the Kalmanfilter for determining the sate of the body.
 2. The method according toclaim 1, wherein the center {right arrow over (m)}_(n) and the radiusr_(n) are determined in a recursion step starting from starting values{right arrow over (m)}₀ and r₀; it is assumed that the relation |{rightarrow over (x)}_(i)-{right arrow over (m)}_(v)|≦r_(v) has been met forall i≦v; if |{right arrow over (x)}_(v+1)-{right arrow over(m)}_(v)|≦r_(v) also applies, {right arrow over (m)}_(v+1)=m_(v) andr_(v+1)=r_(v) will be set; otherwise, {right arrow over(m)}_(v+1)=α·{right arrow over (m)}_(v)+β·{right arrow over (x)}_(v+1)and r_(v+1)=α·|{right arrow over (x)}_(v+1)-{right arrow over (m)}_(v)|will be set, in which${\alpha = {{{\frac{1}{2} \cdot \left( {1 + \frac{r_{v}}{{{\overset{\rightarrow}{x}}_{v + 1} - {\overset{\rightarrow}{m}}_{v}}}} \right)}\mspace{20mu} {and}\mspace{20mu} \beta} = {\frac{1}{2} \cdot \left( {1 - \frac{r_{v}}{{{\overset{\rightarrow}{x}}_{v + 1} - {\overset{\rightarrow}{m}}_{v}}}} \right)}}};$and the recursion step is repeated for v=0, . . . , n.
 3. The methodaccording to claim 2, wherein: the starting values {right arrow over(m)}₀ o and r₀ are defined by determining a k-dimensional cuboid fromthe minimal coordinates k_(min) _(—) _(y) and the maximal coordinatesk_(max) _(—) _(y) with y=1, . . . , k of all points {right arrow over(x)}_(i) in the k dimensions; and a k-dimensional sphere is determined,with radius r₀ and center {right arrow over (x)}₀, {right arrow over(m)}₀ being the center vector of the k-dimensional cuboid and r₀ beinghalf of the longest section from the quantity of coordinate pairsk_(min) _(—) _(y) and k_(max) _(—) _(y) in every dimension.
 4. Themethod according to claim 2, wherein {right arrow over (m)}₀, r₀, theaverage value and the standard deviation of the point quantity {{rightarrow over (x)}_(i), i=1, . . . , n} are selected as the startingvalues;{right arrow over (m)}₀=/n·Σ _(i=1) ^(n) {right arrow over (x)}; and$r_{0} = {{\sqrt{\frac{1}{n}{\cdot {\sum\limits_{i = 1}^{n}{{{\overset{\rightarrow}{x}}_{i} - {\overset{\rightarrow}{m}}_{0}}}^{2}}}}\; {{brz}.\mspace{11mu} r_{0}}} = {\sqrt{\frac{1}{n - 1} \cdot {\sum\limits_{i = 1}^{n}{{{\overset{\rightarrow}{x}}_{i} - {\overset{\rightarrow}{m}}_{0}}}^{2}}}.}}$5. A system for determining the physical state of a body, said systemcomprising: a plurality of sensors which generate a number n of measuredvalues {right arrow over (x)}_(i) for physical quantities that areindicative of states of the body, with i=1 . . . n; a computing devicewhich processes the measured values of a state in each case and computesa first quantity {right arrow over (m)}_(n) and a second quantity r_(n),the quantity {right arrow over (m)}_(n) being a center point vector andthe quantity r_(n) being a radius of a k-dimensional sphere B_(n) withinwhich all points {right arrow over (x)}_(i) with i=1, . . . , n aresituated; and a Kalman filter which is coupled to the computing deviceto receive the computed first and second quantities, which are processedtherein to generate an output that is indicative of the physical stateof the body.
 6. The system according to claim 5, wherein the sensorscomprise at least one of inertia sensors, Terrain Referenced Navigation,radar altimeters, Doppler radars, air data sensors, satellite navigationreceivers and sensors for the determination of yaw angle, inclinationangle or tilting angle.
 7. The method according to claim 1, wherein thephysical state comprises at least one of i) position, altitude or speedof the body, ii) pressure, temperature and iii) humidity.